
#include <fstream>
#include <string>
#include "gflags/gflags.h"
#include "glog/logging.h"
//#include "gpb_util.h"
#include "gps.pb.h"
#include "gpx_parser.h"
#include "kml/base/math_util.h"

DEFINE_string(ingpx, "", "file.gpx");
DEFINE_string(waypoint_name, "", "waypoint name");
DEFINE_int32(dist, 10, "distance from waypoint");

bool FindWayPoint(const gps_proto::GpsProto& gps_proto,
                  const std::string& waypoint_name,
                  gps_proto::PointProto* waypoint_proto) {
  for (int w = 0; w < gps_proto.waypoint_size(); ++w) {
    if (waypoint_name.compare(gps_proto.waypoint(w).name()) == 0) {
      waypoint_proto->CopyFrom(gps_proto.waypoint(w));
      return true;
    }
  }
  return false;
}

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, false);
  if (FLAGS_ingpx.empty()) {
    LOG(ERROR) << "missing arg --ingpx file.gpx";
    return 1;
  }
  if (FLAGS_waypoint_name.empty()) {
    LOG(ERROR) << "missing arg --waypoint_name waypoint-name";
    return 1;
  }
  
  gps_proto::GpsProto gps_proto;
  if (!gps_proto::GpxParser::ParseFromFile(FLAGS_ingpx.c_str(), &gps_proto)) {
    LOG(ERROR) << "parse failed: " << FLAGS_ingpx;
    return 1;
  }

  gps_proto::PointProto waypoint_proto;
  if (!FindWayPoint(gps_proto, FLAGS_waypoint_name, &waypoint_proto)) {
    LOG(ERROR) << "no <wpt> named " << FLAGS_waypoint_name;
    return 1;
  }

  LOG(INFO) << "<track> size " << gps_proto.track_size();
  for (int t = 0; t < gps_proto.track_size(); ++t) {
    LOG(INFO) << "<segment> size " << gps_proto.track(t).segment_size();
    for (int s = 0; s < gps_proto.track(t).segment_size(); ++s) {
      LOG(INFO) << "<point> size "
                << gps_proto.track(t).segment(s).point_size();
      for (int p = 0; p < gps_proto.track(t).segment(s).point_size(); ++p) {
        const gps_proto::PointProto& point =
            gps_proto.track(t).segment(s).point(p);
        double distance = kmlbase::DistanceBetweenPoints(point.lat(),
                                                         point.lon(),
                                                         waypoint_proto.lat(),
                                                         waypoint_proto.lon());
        if (FLAGS_dist > distance) {
          LOG(INFO) << "track " << t << ", seg " << s << ", pt " << p
                    << ", dist " << distance;
        }
      }
    }
  }
  return 0;
}

